/*
  ******************************************************************************
  * File Name          : ieee1459.c
  * Description        : This file provides code for the ieee1459 algorithm.
  * Author             : jackwang by jiawang16@foxmail.com
  * Date               : 2021-03-29
  * Version            : 1.0.0
  ******************************************************************************
*/

/*! include headers */
/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
#include "ieee1459.h"

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*! define private debug macro */
#define _DEBUG_ 0

#if _DEBUG_
    #define MSG(...) DEBUG_Printf(__VA_ARGS__)
#else
    #define MSG(...)
#endif

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*! private radians and angle convert macro */
#define radians_to_angle(a)                         ( (180.0f/PI) * (a) )
#define angle_to_radians(a)                         ( (PI/180.0f) * (a) )

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*! private variable defination */
fft_point_t Ua_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t Ub_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t Uc_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t Ia_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t Ib_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t Ic_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t In_fft_out_buff[MAX_Harmonic_Idx]      __attribute__((section("EXRAM")));
fft_point_t Uab_fft_out_buff[MAX_Harmonic_Idx]     __attribute__((section("EXRAM")));
fft_point_t Uac_fft_out_buff[MAX_Harmonic_Idx]     __attribute__((section("EXRAM")));
fft_point_t Ubc_fft_out_buff[MAX_Harmonic_Idx]     __attribute__((section("EXRAM")));

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*! public variable defination */
fft_data_handle_t fft_data_handle;

float32_t adcBuff_Ua[Block_Size]                   __attribute__((section("EXRAM")));
float32_t adcBuff_Ub[Block_Size]                   __attribute__((section("EXRAM")));
float32_t adcBuff_Uc[Block_Size]                   __attribute__((section("EXRAM")));
float32_t adcBuff_Uab[Block_Size]                  __attribute__((section("EXRAM")));
float32_t adcBuff_Uac[Block_Size]                  __attribute__((section("EXRAM")));
float32_t adcBuff_Ubc[Block_Size]                  __attribute__((section("EXRAM")));
float32_t adcBuff_Ia[Block_Size]                   __attribute__((section("EXRAM")));
float32_t adcBuff_Ib[Block_Size]                   __attribute__((section("EXRAM")));
float32_t adcBuff_Ic[Block_Size]                   __attribute__((section("EXRAM")));
float32_t adcBuff_In[Block_Size]                   __attribute__((section("EXRAM")));
/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_data_handle_init( fft_data_handle_t *pHandle)
{
    pHandle->Ua_fft_out  = Ua_fft_out_buff;
    pHandle->Ub_fft_out  = Ub_fft_out_buff;
    pHandle->Uc_fft_out  = Uc_fft_out_buff;
    pHandle->Ia_fft_out  = Ia_fft_out_buff;
    pHandle->Ib_fft_out  = Ib_fft_out_buff;
    pHandle->Ic_fft_out  = Ic_fft_out_buff;
    pHandle->In_fft_out  = In_fft_out_buff;
    pHandle->Uab_fft_out = Uab_fft_out_buff;
    pHandle->Uac_fft_out = Uac_fft_out_buff;
    pHandle->Ubc_fft_out = Ubc_fft_out_buff;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_calculate_line_voltage(const float32_t* pSrcA, const float32_t* pSrcB, float32_t* pDst, uint32_t blockSize)
{
    #ifdef ARM_MATH_CM4
        arm_sub_f32((float32_t*)pSrcA, (float32_t*)pSrcB, pDst, blockSize);
    #else
        for(uint32_t idx = 0; idx < blockSize; idx++){
            *(pDst+idx) = *(pSrcA+idx) - *(pSrcB+idx);
        }
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_calculate_rms(const float32_t* pSrcA, uint32_t blockSize, float32_t* pResult)
{
    #ifdef ARM_MATH_CM4
        arm_rms_f32((float32_t*)pSrcA, blockSize, pResult);
    #else
        float innerVal = 0;
        for(uint32_t idx = 0; idx < blockSize; idx++){
            innerVal += (*(pSrcA+idx)) * (*(pSrcA+idx));
        }
        
        innerVal /= blockSize;
        
        innerVal = sqrt(innerVal);
        
        *pResult = innerVal;
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_normal_active_power(const float32_t SrcV, const float32_t SrcI, float32_t* pResult)
{
    *pResult = SrcV * SrcI;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_caculate_fundamental_result(const fft_data_handle_t* pHandle, fundamental_result_t* pResult)
{

    #ifdef ARM_MATH_CM4
        pResult->Pa_1 = pHandle->Ua_fft_out[Fundamental_Idx].amp * pHandle->Ia_fft_out[Fundamental_Idx].amp
                        * arm_cos_f32( angle_to_radians(pHandle->Ua_fft_out[Fundamental_Idx].pha - pHandle->Ia_fft_out[Fundamental_Idx].pha) );
        pResult->Pb_1 = pHandle->Ub_fft_out[Fundamental_Idx].amp * pHandle->Ib_fft_out[Fundamental_Idx].amp 
                        * arm_cos_f32( angle_to_radians(pHandle->Ub_fft_out[Fundamental_Idx].pha - pHandle->Ib_fft_out[Fundamental_Idx].pha) );
        pResult->Pc_1 = pHandle->Uc_fft_out[Fundamental_Idx].amp * pHandle->Ic_fft_out[Fundamental_Idx].amp 
                        * arm_cos_f32( angle_to_radians(pHandle->Uc_fft_out[Fundamental_Idx].pha - pHandle->Ic_fft_out[Fundamental_Idx].pha) );
                                                                                                                       
           
        pResult->Qa_1 = pHandle->Ua_fft_out[Fundamental_Idx].amp * pHandle->Ia_fft_out[Fundamental_Idx].amp 
                        * arm_sin_f32( angle_to_radians(pHandle->Ua_fft_out[Fundamental_Idx].pha - pHandle->Ia_fft_out[Fundamental_Idx].pha) );
        pResult->Qb_1 = pHandle->Ub_fft_out[Fundamental_Idx].amp * pHandle->Ib_fft_out[Fundamental_Idx].amp 
                        * arm_sin_f32( angle_to_radians(pHandle->Ub_fft_out[Fundamental_Idx].pha - pHandle->Ib_fft_out[Fundamental_Idx].pha) );
        pResult->Qc_1 = pHandle->Uc_fft_out[Fundamental_Idx].amp * pHandle->Ic_fft_out[Fundamental_Idx].amp 
                        * arm_sin_f32( angle_to_radians(pHandle->Uc_fft_out[Fundamental_Idx].pha - pHandle->Ic_fft_out[Fundamental_Idx].pha) );
    #else
        pResult->Pa_1 = pHandle->Ua_fft_out[Fundamental_Idx].amp * pHandle->Ia_fft_out[Fundamental_Idx].amp 
                        * cos( angle_to_radians(pHandle->Ua_fft_out[Fundamental_Idx].pha - pHandle->Ia_fft_out[Fundamental_Idx].pha) );
        pResult->Pb_1 = pHandle->Ub_fft_out[Fundamental_Idx].amp * pHandle->Ib_fft_out[Fundamental_Idx].amp 
                        * cos( angle_to_radians(pHandle->Ub_fft_out[Fundamental_Idx].pha - pHandle->Ib_fft_out[Fundamental_Idx].pha) );
        pResult->Pc_1 = pHandle->Uc_fft_out[Fundamental_Idx].amp * pHandle->Ic_fft_out[Fundamental_Idx].amp 
                        * cos( angle_to_radians(pHandle->Uc_fft_out[Fundamental_Idx].pha - pHandle->Ic_fft_out[Fundamental_Idx].pha) );
                                                                                                                       
           
        pResult->Qa_1 = pHandle->Ua_fft_out[Fundamental_Idx].amp * pHandle->Ia_fft_out[Fundamental_Idx].amp 
                        * sin( angle_to_radians(pHandle->Ua_fft_out[Fundamental_Idx].pha - pHandle->Ia_fft_out[Fundamental_Idx].pha) );
        pResult->Qb_1 = pHandle->Ub_fft_out[Fundamental_Idx].amp * pHandle->Ib_fft_out[Fundamental_Idx].amp 
                        * sin( angle_to_radians(pHandle->Ub_fft_out[Fundamental_Idx].pha - pHandle->Ib_fft_out[Fundamental_Idx].pha) );
        pResult->Qc_1 = pHandle->Uc_fft_out[Fundamental_Idx].amp * pHandle->Ic_fft_out[Fundamental_Idx].amp 
                        * sin( angle_to_radians(pHandle->Uc_fft_out[Fundamental_Idx].pha - pHandle->Ic_fft_out[Fundamental_Idx].pha) );
    #endif
    
    pResult->P_1    = pResult->Pa_1 + pResult->Pb_1 + pResult->Pc_1;
    pResult->Q_1    = pResult->Qa_1 + pResult->Qb_1 + pResult->Qc_1;

	#ifdef ARM_MATH_CM4
		arm_sqrt_f32( (pResult->P_1 * pResult->P_1) + (pResult->Q_1 * pResult->Q_1) , &pResult->S_1);
	#else
		pResult->S_1= sqrt( (pResult->P_1 * pResult->P_1) + (pResult->Q_1 * pResult->Q_1) );
	#endif
    
	
    pResult->Sa_1   = pHandle->Ua_fft_out[Fundamental_Idx].amp * pHandle->Ia_fft_out[Fundamental_Idx].amp;
    pResult->Sb_1   = pHandle->Ub_fft_out[Fundamental_Idx].amp * pHandle->Ib_fft_out[Fundamental_Idx].amp;
    pResult->Sc_1   = pHandle->Uc_fft_out[Fundamental_Idx].amp * pHandle->Ic_fft_out[Fundamental_Idx].amp;

	
    pResult->factor = pResult->P_1 / pResult->S_1;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_caculate_sequence_positive(const fft_point_t srcA, const fft_point_t srcB, const fft_point_t srcC, fft_point_t* pResult)
{
    float32_t innerVal_a = 0, innerVal_b = 0;
    float32_t seta;/*! radians */
    
    #ifdef ARM_MATH_CM4
        innerVal_a += srcA.amp * arm_cos_f32( angle_to_radians( srcA.pha ) );
        innerVal_a += srcB.amp * arm_cos_f32( angle_to_radians( srcB.pha + 120.0f) );
        innerVal_a += srcC.amp * arm_cos_f32( angle_to_radians( srcC.pha + 240.0f) );
    
        innerVal_b += srcA.amp * arm_sin_f32( angle_to_radians( srcA.pha ) );
        innerVal_b += srcB.amp * arm_sin_f32( angle_to_radians( srcB.pha + 120.0f) );
        innerVal_b += srcC.amp * arm_sin_f32( angle_to_radians( srcC.pha + 240.0f) );
    #else
        innerVal_a += srcA.amp * cos( angle_to_radians( srcA.pha ) );
        innerVal_a += srcB.amp * cos( angle_to_radians( srcB.pha + 120.0f) );
        innerVal_a += srcC.amp * cos( angle_to_radians( srcC.pha + 240.0f) );
                                                      
        innerVal_b += srcA.amp * sin( angle_to_radians( srcA.pha ) );
        innerVal_b += srcB.amp * sin( angle_to_radians( srcB.pha + 120.0f) );
        innerVal_b += srcC.amp * sin( angle_to_radians( srcC.pha + 240.0f) );
    #endif
        
    seta = atan(innerVal_b / innerVal_a);
    
    pResult->pha = radians_to_angle(seta);
    
    innerVal_a  = innerVal_a * innerVal_a;
    innerVal_b  = innerVal_b * innerVal_b;
    
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( 1/9.0f * (innerVal_a + innerVal_b), &pResult->amp);
    #else
        pResult->amp = 1/3.0f * sqrt( innerVal_a + innerVal_b );
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_caculate_sequence_negtive(const fft_point_t srcA, const fft_point_t srcB, const fft_point_t srcC, fft_point_t* pResult)
{
    float32_t innerVal_a = 0, innerVal_b = 0;
    float32_t seta;/*! radians */
    
    #ifdef ARM_MATH_CM4
        innerVal_a += srcA.amp * arm_cos_f32( angle_to_radians( srcA.pha ) );
        innerVal_a += srcB.amp * arm_cos_f32( angle_to_radians( srcB.pha + 240.0f) );
        innerVal_a += srcC.amp * arm_cos_f32( angle_to_radians( srcC.pha + 120.0f) );
    
        innerVal_b += srcA.amp * arm_sin_f32( angle_to_radians( srcA.pha ) );
        innerVal_b += srcB.amp * arm_sin_f32( angle_to_radians( srcB.pha + 240.0f) );
        innerVal_b += srcC.amp * arm_sin_f32( angle_to_radians( srcC.pha + 120.0f) );
    #else
        innerVal_a += srcA.amp * cos( angle_to_radians( srcA.pha ) );
        innerVal_a += srcB.amp * cos( angle_to_radians( srcB.pha + 240.0f) );
        innerVal_a += srcC.amp * cos( angle_to_radians( srcC.pha + 120.0f) );
                                                      
        innerVal_b += srcA.amp * sin( angle_to_radians( srcA.pha ) );
        innerVal_b += srcB.amp * sin( angle_to_radians( srcB.pha + 240.0f) );
        innerVal_b += srcC.amp * sin( angle_to_radians( srcC.pha + 120.0f) );
    #endif
        
    seta = atan(innerVal_b / innerVal_a);
    
    pResult->pha = radians_to_angle(seta);
    
    innerVal_a  = innerVal_a * innerVal_a;
    innerVal_b  = innerVal_b * innerVal_b;
    
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( 1/9.0f * (innerVal_a + innerVal_b), &pResult->amp);
    #else
        pResult->amp = 1/3.0f * sqrt( innerVal_a + innerVal_b );
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_caculate_sequence_zero(const fft_point_t srcA, const fft_point_t srcB, const fft_point_t srcC, fft_point_t* pResult)
{
    float32_t innerVal_a = 0, innerVal_b = 0;
    float32_t seta;/*! radians */
    
    #ifdef ARM_MATH_CM4
        innerVal_a += srcA.amp * arm_cos_f32( angle_to_radians( srcA.pha ) );
        innerVal_a += srcB.amp * arm_cos_f32( angle_to_radians( srcB.pha ) );
        innerVal_a += srcC.amp * arm_cos_f32( angle_to_radians( srcC.pha ) );
    
        innerVal_b += srcA.amp * arm_sin_f32( angle_to_radians( srcA.pha ) );
        innerVal_b += srcB.amp * arm_sin_f32( angle_to_radians( srcB.pha ) );
        innerVal_b += srcC.amp * arm_sin_f32( angle_to_radians( srcC.pha ) );
    #else
        innerVal_a += srcA.amp * cos( angle_to_radians( srcA.pha ) );
        innerVal_a += srcB.amp * cos( angle_to_radians( srcB.pha ) );
        innerVal_a += srcC.amp * cos( angle_to_radians( srcC.pha ) );
                                                      
        innerVal_b += srcA.amp * sin( angle_to_radians( srcA.pha ) );
        innerVal_b += srcB.amp * sin( angle_to_radians( srcB.pha ) );
        innerVal_b += srcC.amp * sin( angle_to_radians( srcC.pha ) );
    #endif
        
    seta = atan(innerVal_b / innerVal_a);
    
    pResult->pha = radians_to_angle(seta);
    
    innerVal_a  = innerVal_a * innerVal_a;
    innerVal_b  = innerVal_b * innerVal_b;
    
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( 1/9.0f * (innerVal_a + innerVal_b), &pResult->amp);
    #else
        pResult->amp = 1/3.0f * sqrt( innerVal_a + innerVal_b );
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_sequence_result(const fft_data_handle_t *pHandle, sequence_result_t *pResult)
{
    IEEE1459_caculate_sequence_positive(pHandle->Ua_fft_out[Fundamental_Idx], pHandle->Ub_fft_out[Fundamental_Idx],
                                        pHandle->Uc_fft_out[Fundamental_Idx], &pResult->Up_1);
    
    IEEE1459_caculate_sequence_negtive(pHandle->Ua_fft_out[Fundamental_Idx], pHandle->Ub_fft_out[Fundamental_Idx],
                                       pHandle->Uc_fft_out[Fundamental_Idx], &pResult->Un_1);
    
    IEEE1459_caculate_sequence_zero(pHandle->Ua_fft_out[Fundamental_Idx], pHandle->Ub_fft_out[Fundamental_Idx],
                                    pHandle->Uc_fft_out[Fundamental_Idx], &pResult->Uz_1);
    
    IEEE1459_caculate_sequence_positive(pHandle->Ia_fft_out[Fundamental_Idx], pHandle->Ib_fft_out[Fundamental_Idx],
                                        pHandle->Ic_fft_out[Fundamental_Idx], &pResult->Ip_1);
    
    IEEE1459_caculate_sequence_negtive(pHandle->Ia_fft_out[Fundamental_Idx], pHandle->Ib_fft_out[Fundamental_Idx],
                                       pHandle->Ic_fft_out[Fundamental_Idx], &pResult->In_1);
    
    IEEE1459_caculate_sequence_zero(pHandle->Ia_fft_out[Fundamental_Idx], pHandle->Ib_fft_out[Fundamental_Idx],
                                    pHandle->Ic_fft_out[Fundamental_Idx], &pResult->Iz_1);
    
    pResult->pha = pResult->Up_1.pha - pResult->Ip_1.pha;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void 
IEEE1459_caculate_sequence_positive_power(const sequence_result_t *pResult, sequence_power_t* pPower)
{
    #ifdef ARM_MATH_CM4
        pPower->Pp_1 = 3.0f * pResult->Up_1.amp * pResult->Ip_1.amp * arm_cos_f32( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha ) );
        pPower->Qp_1 = 3.0f * pResult->Up_1.amp * pResult->Ip_1.amp * arm_sin_f32( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha ) );
        arm_sqrt_f32( ( (pPower->Pp_1 * pPower->Pp_1) * (pPower->Qp_1 * pPower->Qp_1) ), &pPower->Sp_1);
    #else
        pPower->Pp_1 = 3.0f * pResult->Up_1.amp * pResult->Ip_1.amp * cos( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha ) );
        pPower->Qp_1 = 3.0f * pResult->Up_1.amp * pResult->Ip_1.amp * sin( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha ) );
        pPower->Sp_1 = sqrt( (pPower->Pp_1 * pPower->Pp_1) + (pPower->Qp_1 * pPower->Qp_1) );
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_sequence_current(const sequence_result_t *pResult, sequence_current_t* pCurrent)
{
    #ifdef ARM_MATH_CM4
        pCurrent->Ip_P_1 = pResult->Ip_1.amp * arm_cos_f32( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha) );
        pCurrent->Ip_Q_1 = pResult->Ip_1.amp * arm_sin_f32( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha) );
        arm_sqrt_f32( ( (pResult->In_1.amp * pResult->In_1.amp) + 4*(pResult->Iz_1.amp * pResult->Iz_1.amp) ), &pCurrent->Ip_U_1);
    #else
        pCurrent->Ip_P_1 = pResult->Ip_1.amp * cos( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha) );
        pCurrent->Ip_Q_1 = pResult->Ip_1.amp * sin( angle_to_radians( pResult->Up_1.pha - pResult->Ip_1.pha) );
        pCurrent->Ip_U_1 = sqrt( ( (pResult->In_1.amp * pResult->In_1.amp) + 4*(pResult->Iz_1.amp * pResult->Iz_1.amp) ));
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_harmonic_current(const float32_t Ix,const float32_t Ix_1, float32_t* pRestlt)
{
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( ( (Ix * Ix) - (Ix_1 * Ix_1) ), pRestlt);
    #else
        *pRestlt = sqrt( ( (Ix * Ix) - (Ix_1 * Ix_1) ) );
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_total_harmonic_current(const harmonic_current_t* pCurrent, float32_t* pResult)
{
    float32_t innerVal = 0;
    
    innerVal += ( pCurrent->Ia_h * pCurrent->Ia_h );
    innerVal += ( pCurrent->Ib_h * pCurrent->Ib_h );
    innerVal += ( pCurrent->Ic_h * pCurrent->Ic_h );
    
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( innerVal, pResult);
    #else
        *pResult = sqrt(innerVal);
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_equivalent_apparent_power(const equivalent_apparent_power_paras_t* pParas, float32_t* pResult)
{
    float32_t innerVal = 0;
    float32_t Ue = 0;
    float32_t Ie = 0;
    
    /*! caculate U */
    innerVal += (pParas->Ua * pParas->Ua);
    innerVal += (pParas->Ub * pParas->Ub);
    innerVal += (pParas->Uc * pParas->Uc);
    innerVal *= 3; 
    
    innerVal += (pParas->Uab * pParas->Uab);
    innerVal += (pParas->Uac * pParas->Uac);
    innerVal += (pParas->Ubc * pParas->Ubc);
    
    innerVal /= 18.0f;
    
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( innerVal, &Ue);
    #else
        Ue = sqrt(innerVal);
    #endif
    
    /*! caculate I */
    innerVal  = 0;
    innerVal += (pParas->Ia * pParas->Ia);
    innerVal += (pParas->Ib * pParas->Ib);
    innerVal += (pParas->Ic * pParas->Ic);
    innerVal += (pParas->In * pParas->In);
    innerVal /= 3.0f;
    
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( innerVal, &Ie);
    #else
        Ie = sqrt(innerVal);
    #endif
    
    /*! output */
    *pResult = 3.0f * Ue * Ie;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_unbalance_equivalent_apparent_power(const float32_t Se_1, const float32_t Sp_1, float32_t* pResult)
{
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( ((Se_1 * Se_1) - (Sp_1 * Sp_1)), pResult);
    #else
        *pResult = sqrt(((Se_1 * Se_1) - (Sp_1 * Sp_1)));
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_harmonic_equivalent_apparent_power(const float32_t Se, const float32_t Se_1, float32_t* pResult)
{
    #ifdef ARM_MATH_CM4
        arm_sqrt_f32( ((Se * Se) - (Se_1 * Se_1)), pResult);
    #else
        *pResult = sqrt(((Se * Se) - (Se_1 * Se_1)));
    #endif
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_equivalent_power_factor(const float32_t P, const float32_t Se, float32_t* pResult)
{
    *pResult = P / Se;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_fundamental_power_factor(const float32_t Pp_1, const float32_t Sp_1, float32_t* pResult)
{
    *pResult = Pp_1 / Sp_1;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_effictive_power_factor(const float32_t Pp_1, const float32_t Se, float32_t* pResult)
{
    *pResult = Pp_1 / Se;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_harmonic_pollute_factor(const float32_t Se_N, const float32_t Se_1, float32_t* pResult)
{
    *pResult = Se_N / Se_1;
}

/*!~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void
IEEE1459_caculate_all(const fft_data_handle_t* pHandle, ieee1459_result_t* pResult)
{
    IEEE1459_calculate_line_voltage(adcBuff_Ua, adcBuff_Ub, adcBuff_Uab, Block_Size);
    IEEE1459_calculate_line_voltage(adcBuff_Ua, adcBuff_Uc, adcBuff_Uac, Block_Size);
    IEEE1459_calculate_line_voltage(adcBuff_Ub, adcBuff_Uc, adcBuff_Ubc, Block_Size);

    IEEE1459_calculate_rms(adcBuff_Ua,  Block_Size, &pResult->rms.Ua);
    IEEE1459_calculate_rms(adcBuff_Ub,  Block_Size, &pResult->rms.Ub);
    IEEE1459_calculate_rms(adcBuff_Uc,  Block_Size, &pResult->rms.Uc);
    IEEE1459_calculate_rms(adcBuff_Uab, Block_Size, &pResult->rms.Uab);
    IEEE1459_calculate_rms(adcBuff_Uac, Block_Size, &pResult->rms.Uac);
    IEEE1459_calculate_rms(adcBuff_Ubc, Block_Size, &pResult->rms.Ubc);
    IEEE1459_calculate_rms(adcBuff_Ia,  Block_Size, &pResult->rms.Ia);
    IEEE1459_calculate_rms(adcBuff_Ib,  Block_Size, &pResult->rms.Ib);
    IEEE1459_calculate_rms(adcBuff_Ic,  Block_Size, &pResult->rms.Ic);
    IEEE1459_calculate_rms(adcBuff_In,  Block_Size, &pResult->rms.In);

    IEEE1459_caculate_normal_active_power(pResult->rms.Ua, pResult->rms.Ia, &pResult->normal_active_power.Pa);
    IEEE1459_caculate_normal_active_power(pResult->rms.Ub, pResult->rms.Ib, &pResult->normal_active_power.Pb);
    IEEE1459_caculate_normal_active_power(pResult->rms.Uc, pResult->rms.Ic, &pResult->normal_active_power.Pc);
    pResult->normal_active_power.Pall = pResult->normal_active_power.Pa + pResult->normal_active_power.Pb + pResult->normal_active_power.Pc;

    IEEE1459_caculate_fundamental_result(pHandle, &pResult->fundamental_result);

    sequence_result_t sequence_result;
    IEEE1459_caculate_sequence_result(pHandle, &sequence_result);

    sequence_current_t sequence_current;
    IEEE1459_caculate_sequence_current(&sequence_result, &sequence_current);

    IEEE1459_caculate_harmonic_current(pResult->rms.Ia, pHandle->Ia_fft_out[Fundamental_Idx].amp, &pResult->harmonic_current.Ia_h);
    IEEE1459_caculate_harmonic_current(pResult->rms.Ib, pHandle->Ib_fft_out[Fundamental_Idx].amp, &pResult->harmonic_current.Ib_h);
    IEEE1459_caculate_harmonic_current(pResult->rms.Ic, pHandle->Ic_fft_out[Fundamental_Idx].amp, &pResult->harmonic_current.Ic_h);
    IEEE1459_caculate_total_harmonic_current(&pResult->harmonic_current, &pResult->harmonic_current.Iall_h);


    IEEE1459_caculate_sequence_positive_power(&sequence_result, &pResult->sequence_power);

    equivalent_apparent_power_paras_t paras;
    paras.Ua = pResult->rms.Ua;
    paras.Ub = pResult->rms.Ub;
    paras.Uc = pResult->rms.Uc;
    paras.Ia = pResult->rms.Ia;
    paras.Ib = pResult->rms.Ib;
    paras.Ic = pResult->rms.Ic;
    paras.Uab = pResult->rms.Uab;
    paras.Uac = pResult->rms.Uac;
    paras.Ubc = pResult->rms.Ubc;
    paras.In  = pResult->rms.In;
    IEEE1459_caculate_equivalent_apparent_power(&paras, &pResult->equivalent_paras.Se);

    paras.Ua = pHandle->Ua_fft_out[Fundamental_Idx].amp;
    paras.Ub = pHandle->Ub_fft_out[Fundamental_Idx].amp;
    paras.Uc = pHandle->Uc_fft_out[Fundamental_Idx].amp;
    paras.Ia = pHandle->Ia_fft_out[Fundamental_Idx].amp;
    paras.Ib = pHandle->Ib_fft_out[Fundamental_Idx].amp;
    paras.Ic = pHandle->Ic_fft_out[Fundamental_Idx].amp;
    paras.Uab = pHandle->Uab_fft_out[Fundamental_Idx].amp;
    paras.Uac = pHandle->Uac_fft_out[Fundamental_Idx].amp;
    paras.Ubc = pHandle->Ubc_fft_out[Fundamental_Idx].amp;
    paras.In  = pHandle->In_fft_out[Fundamental_Idx].amp;
    IEEE1459_caculate_equivalent_apparent_power(&paras, &pResult->equivalent_paras.Se_1);

    IEEE1459_caculate_harmonic_equivalent_apparent_power(pResult->equivalent_paras.Se, pResult->equivalent_paras.Se_1, &pResult->equivalent_paras.Se_N);

    IEEE1459_caculate_harmonic_equivalent_apparent_power(pResult->equivalent_paras.Se_1, pResult->sequence_power.Sp_1, &pResult->equivalent_paras.Su_1);

    IEEE1459_caculate_equivalent_power_factor(pResult->normal_active_power.Pall, pResult->equivalent_paras.Se, &pResult->equivalent_paras.PFe);

    IEEE1459_caculate_fundamental_power_factor(pResult->sequence_power.Pp_1, pResult->sequence_power.Sp_1, &pResult->equivalent_paras.PFp_1);

    IEEE1459_caculate_effictive_power_factor(pResult->sequence_power.Pp_1, pResult->equivalent_paras.Se,&pResult->equivalent_paras.PFp_e);

    IEEE1459_caculate_harmonic_pollute_factor(pResult->equivalent_paras.Se_N, pResult->equivalent_paras.Se_1, &pResult->equivalent_paras.Hpol);

}

/*! end of file */